#include <Wire.h>
#include <BLDC.h>
#include <Comm.h>
#include <pid.h>


// use the same buf for read/write
// read frame size:23 bytes; write frame size:9 bytes
#define DF_SIZE_OUT 22
#define DF_SIZE_IN 8
#define DF_SIZE_IN_I2C  2
#define DF_SIZE_OUT_I2C 6

uint8_t g_df_buf_out[DF_SIZE_OUT];  
uint8_t g_df_buf_in[DF_SIZE_IN];
uint8_t g_df_buf_i2c_in[DF_SIZE_IN_I2C];
DataFrame g_df_in, g_df_i2c_in;
PID pid;


void update_df_buf_out() {
  S16D1ToBs(&(g_df_buf_out[0]), BLDC_ReadSpeed());
//  S16D1ToBs(&(g_df_buf_out[2]), pid.kp);
  S16D1ToBs(&(g_df_buf_out[2]), pid.target+0.2);
  S16D1ToBs(&(g_df_buf_out[4]), pid.ki);
  S16D1ToBs(&(g_df_buf_out[6]), pid.kd);
  S16D1ToBs(&(g_df_buf_out[8]), pid.p_item);  // not impletmented yet
  S16D1ToBs(&(g_df_buf_out[10]), pid.i_item);
  S16D1ToBs(&(g_df_buf_out[12]), pid.d_item);
  S16D1ToBs(&(g_df_buf_out[14]), BLDC_ReadSysVolt());
  
  uint16_t _ia, _ib, _ic;
  BLDC_ReadCurrent(&_ia, &_ib, &_ic);  
  U16ToBs(&(g_df_buf_out[16]), _ia);
  U16ToBs(&(g_df_buf_out[18]), _ib);
  U16ToBs(&(g_df_buf_out[20]), _ic);
}

void parse_and_apply(bool parse_i2c_buf=false) {
  uint8_t *_buf = parse_i2c_buf ? g_df_i2c_in.buf : g_df_in.buf;

  float _speed = bsToS16D1(_buf, 0);
  if (_speed < 0) {
    BLDC_SetDir(MotorBackward);
    pid.target = - _speed;
  }
  else if (_speed > 0) {
    BLDC_SetDir(MotorForward); 
    pid.target = _speed;
  }
  else {   // _speed equals to 0
    pid.ClearHistory();
  } 
}

void write_frame_uart() {
  update_df_buf_out();
  Serial.write((uint8_t)DF_HEAD);
  Serial.write(g_df_buf_out, sizeof(g_df_buf_out));
}


void write_frame_i2c() {
  S16D1ToBs(&(g_df_buf_out[0]), BLDC_ReadSpeed());
  S16D1ToBs(&(g_df_buf_out[2]), pid.target);
  S16D1ToBs(&(g_df_buf_out[4]), BLDC_ReadSysVolt());
  Wire.write((uint8_t)DF_HEAD);
  Wire.write(g_df_buf_out, DF_SIZE_OUT_I2C);
  Serial.write("write to i2c\r\n");  // debug
}


void read_frame_uart() {
  if (Serial.available() < DF_SIZE_IN) {return;}
  
  while (Serial.available() > 0) {
    int ch = Serial.read();
    if (RECV_DONE == DF_Feed(&g_df_in, ch)){
      parse_and_apply();
    }
  }
}


void read_frame_i2c(int n_received) {
  delay(10);
  Serial.print("recv i2c, available: ");  // debug
  Serial.println(Wire.available());

  while (Wire.available()) {
    int ch = Wire.read();
    if (RECV_DONE == DF_Feed(&g_df_i2c_in, ch)){
      parse_and_apply(true);
      Serial.print(pid.target);
      Serial.print(bsToU16D1(g_df_i2c_in.buf, 0));
      Serial.println(" apply received.");  // debug
    }
  }
}


bool TimeUp(){
  static const unsigned int interval = 50;
  static long lastTime = millis();
  if (millis() - lastTime > interval) {
    lastTime = millis();
    return true;
  }
  return false;
}


void setup() {
  Serial.begin(57600);
  Wire.begin(10);   // slave address 10
  Wire.onRequest(write_frame_i2c);
  Wire.onReceive(read_frame_i2c);
  
  BLDC_Init();
  DF_Init(&g_df_in, g_df_buf_in, DF_SIZE_IN, DF_SIZE_IN);
  DF_Init(&g_df_i2c_in, g_df_buf_i2c_in, DF_SIZE_IN_I2C, DF_SIZE_IN_I2C);
  
  pid.kp = 2;
  pid.ki = 1;
}


void loop() {
  // ===========================
  // Routine
  // ===========================
  // switch
  BLDC_Update();  

  // read input
  read_frame_uart();
  
  // report status
  if (TimeUp()) {
    // pid speed control
    pid.Update(BLDC_ReadSpeed());
    if (pid.out >= 0) {
      BLDC_SetPwmDuty(pid.out);
    }
    else {
      BLDC_SetPwmDuty(0);
    }

    // report status
//    write_frame_uart();
  }
}
